import math

class Node:
    def __init__(self,x,y,name,moveCost=0,parent=None):
        self.name = name
        self.parent = parent
        self.moveCost = moveCost
        self.score = 0
        self.neighbours = []
        self.g_score = 0
        self.h_score = 0
        self.f_score = 0
        self.x = x
        self.y = y

    def __repr__(self):
        return self.name

    def __eq__(self, v):
        if self.name == v.name:
            return True
        else:
            return False


class Path:
    def __init__(self, nodes, totalCost):
        self.totalCost = totalCost
        self.nodes = nodes

    def insertNodes(self, nodes):
        for n in nodes:
            self.nodes.append(n)
            
    def getNodesFromPath(self):
        return self.nodes


    def getPathTotalCost(self):
        return self.totalCost
    
    def __repr__(self):
        return "nodes = " + str(self.nodes) + "\ntotalCost = " + str(self.totalCost)

class Graph:
    def __init__(self, V, E):
        self.V = V
        self.E = E
        for e in E:
            i = self.V.index(e[0])
            self.V[i].neighbours.append(e[1])
            i = self.V.index(e[1])
            self.V[i].neighbours.append(e[0])
        print "vertices = ", self.V
        print "arestas = ", self.E



class AStar:
    def __init__(self, graph):
        self.mapGraph = graph
        print "AStar iniciado"
        for v in self.mapGraph.V:
            v.g_score = 0
            v.h_score = 0
            v.f_score = 0
            
    def getBestOpenNode(self):
        bestNode = None
        for node in self.openNodesList:
            if not bestNode: # pega o  primeiro no
                bestNode = node
            else:
                if bestNode.f_score >= node.f_score:
                    bestNode = node # no com menor score eh escolhido
        return bestNode


    def findPath(self,srcNode, dstNode):
        myPath = Path([],0)
        self.openNodesList = [] # vertices ainda nao avaliados
        self.closeNodesList = [] # vertices ja analisados
    
        self.openNodesList.append(srcNode)
        print "inserindo no", srcNode, " na lista aberta"

        srcNode.g_score = 0
        srcNode.h_score = self.estimate(srcNode, dstNode)
        srcNode.f_score = srcNode.h_score

        for v in self.mapGraph.V:
            if v != srcNode:
                v.g_score = self.distance(srcNode, v)
            print "g_score de ", v, v.g_score

        while len(self.openNodesList) != 0:
            n = self.getBestOpenNode()
            print "pegando no aberto ", n 
            if n == dstNode:
                print "chegou! o caminho eh tal..."
                p = self.tracePath(srcNode,n)
                myPath.insertNodes(p[0])
                myPath.totalCost = p[1]
                print myPath
                return myPath
            else:
                print "Analisarei ", n, "(sera removido dos nos abertos)"
                self.openNodesList.remove(n)
                self.closeNodesList.append(n)
                for v in n.neighbours:
                    tentative_g_score = n.g_score + self.distance(n,v)
                    print "tentative g_score de ", v, tentative_g_score
                    
                    if v in self.closeNodesList:
                        continue
                    
                    elif v not in self.openNodesList:
                        print "adicionando ", v, " a lista de nos abertos"
                        self.openNodesList.append(v)
                        bestChoice = True

                    elif tentative_g_score < v.g_score:
                        bestChoice = True
                    else:
                        bestChoice = False


                    if bestChoice == True:
                        v.parent = n
                        v.g_score = tentative_g_score
                        v.h_score = self.estimate(v, dstNode)
                        v.f_score = v.g_score + v.h_score
                    

    def estimate(self,v1,end):
        ## estimativa do vertice v1 ate o final do caminho
        ## esta sendo usada a distancia euclidiana
        return math.sqrt((end.x-v1.x)**2+(end.y-v1.y)**2)
        

    def distance(self, v1,v2):
        ## TO DO: implementar funcao de distancia entre dois vertices do grafo
        neig = 0
        if v1 == v2:
            return 0
        if v2 in v1.neighbours:
            return 1
        else:    
            for v in v1.neighbours:
                if v2 in v.neighbours:
                    neig = 2
                    break
        if neig == 2:
            return 2
        else: return 3
                
        

    def tracePath(self,v1,v2,cost=0):
        if v2.parent != None:
            p = self.tracePath(v1,v2.parent,cost)
            p[0].append(v2)
            return (p[0],p[1]+1)
        else:
            return ([v1],cost)

        



#########################
if __name__ == '__main__':

    v1 = Node(0,0,"v1")
    v2 = Node(1,0,"v2")
    v3 = Node(2,0,"v3")
    v4 = Node(1,1,"v4")
    v5 = Node(2,1,"v5")
    v6 = Node(3,1,"v6")

    G = Graph([v1,v2,v3,v4,v5,v6],[[v1,v2],[v2,v3],[v2,v4],[v3,v5],[v4,v5],[v5,v6]])

    print v2.neighbours

    coisa = AStar(G)
    coisa.findPath(v1,v6)
